#! /usr/bin/env python

# Copyright (c) 2018, Rethink Robotics Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import rospy
import json
from intera_core_msgs.msg import (
                                   IODeviceStatus,
                                   IODataStatus,
                                   IOComponentCommand,
                                   IOComponentStatus,
                                   IOStatus,
                                   IONodeConfiguration,
                                   IOComponentConfiguration,
                                   IODeviceConfiguration
                                 )

class CameraIOSim(object):
    def __init__(self, name, is_streaming):
        self._name= name
        self._cmd_times = []
        self._state_pub = rospy.Publisher("/io/internal_camera/" + name + "/state",
                                          IODeviceStatus, queue_size=10, latch=True)
        self._config_pub = rospy.Publisher("/io/internal_camera/" + name + "/config",
                                          IODeviceConfiguration, queue_size=10, latch=True)
        self._node_config_pub = rospy.Publisher("io/internal_camera/config",
                                        IONodeConfiguration, queue_size=10, latch=True)
        self._command_sub = rospy.Subscriber("/io/internal_camera/" + name + "/command",
                                             IOComponentCommand, self._handle_command)
        self._node_config_pub.publish(self._construct_node_config())
        self._config_pub.publish(self._construct_config())
        self._state_pub.publish(self._construct_state(is_streaming))

    def _construct_node_config(self):
        msg = IONodeConfiguration()
        msg.time = rospy.get_rostime()
        if not msg.time.secs:
            msg.time.secs = 1  # hack to ensure time is valid

        msg.node.name = 'internal'
        msg.node.config = json.dumps({"params":{},"plugins":{"SawyerInternalCameraPlugin":{"params":{},"props":{},"url":"package:\\/\\/camera_io\\/config\\/sawyer_sdk_internal_camera_plugin.json"}},"props":{"id":"\\/io\\/internal_camera","name":"internal","rev":1,"state_pub_rate_hz":5,"update_rate_hz":100}})

        msg.devices.append(IOComponentConfiguration(name='right_hand_camera',
          config=json.dumps({"params":{"command_service_timeout":1.0,"engine_params":{"autoEnroll":True,"configurable":False},"fps":10,"ftp_host":"port:203.0.113.130","height":480,"image_path":"package:\\/\\/camera_io\\/images\\/cognex","image_source_id":"cognex:00:D0:24:3D:2F:F6","sim_image":"Fiducials_marker1.png","tcp_port":8900,"width":752},"ports":{"set_brightness":{"params":{"control":"brightness"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_camera_resolution":{"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"owner":"device","rev":1,"type":"CameraResolution"}},"set_camera_stream_state":{"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_embedded_data":{"params":{},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"Data"}},"set_exposure":{"params":{"control":"exposure"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_flip":{"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_gain":{"params":{"control":"gain"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_mirror":{"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_strobe":{"params":{"control":"strobe"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlStrobe"}},"set_window_x":{"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_window_y":{"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}}},"props":{"id":"\\/io\\/internal_camera\\/right_hand_camera","name":"right_hand_camera","owner":"device","rev":1,"type":"CognexCameraDevice"},"signals":{"camera_streaming":{"params":{"default":[True]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}},"resolution":{"params":{"default":[{"height":480,"width":752}]},"props":{"format":{"role":"output","type":"object"},"owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}},"set_brightness":{"params":{"default":[10],"range":{"max":[100],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_brightness","type":"RangedIO"}},"set_embedded_data":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_embedded_data","type":"IO"}},"set_exposure":{"params":{"default":[32],"range":{"max":[100],"min":[1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}},"set_flip":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_flip","type":"IO"}},"set_gain":{"params":{"default":[5],"range":{"max":[255],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}},"set_mirror":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_mirror","type":"IO"}},"set_strobe":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_strobe","type":"IO"}},"set_window_x":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_x","type":"IO"}},"set_window_y":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_y","type":"IO"}}}})))
        msg.devices.append(IOComponentConfiguration(name='head_camera',
          config=json.dumps({"params":{"engine_params":{"autoEnroll":True,"configurable":False},"fps":10,"height":800,"image_path":"package:\\/\\/camera_io\\/images\\/ienso","image_source_id":"ienso_ethernet:EE:52:EB:4E:46:B8","sim_image":"Fiducials_marker1.png","tcp_port":8901,"width":1280},"ports":{"set_camera_resolution":{"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"owner":"device","rev":1,"type":"CameraResolution"}},"set_camera_stream_state":{"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_exposure":{"params":{"control":"exposure"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_flip":{"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_gain":{"params":{"control":"gain"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_mirror":{"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_window_x":{"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_window_y":{"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}}},"props":{"id":"\\/io\\/internal_camera\\/head_camera","name":"head_camera","owner":"device","rev":1,"type":"IensoCameraDevice"},"signals":{"camera_streaming":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}},"resolution":{"params":{"default":[{"height":800,"width":1280}]},"props":{"format":{"role":"output","type":"object"},"owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}},"set_exposure":{"params":{"default":[-1],"range":{"max":[100],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}},"set_flip":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_flip","type":"IO"}},"set_gain":{"params":{"default":[0],"range":{"max":[79],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}},"set_mirror":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_mirror","type":"IO"}},"set_window_x":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_x","type":"IO"}},"set_window_y":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_y","type":"IO"}}}})))
        msg.plugins.append(IOComponentConfiguration(name="SawyerInternalCameraPlugin",
          config=json.dumps({"params":{"cameras":[{"driver":"cognex","expected_address":"port:203.0.113.130","name":"right_hand_camera","type":"CognexCameraDevice"},{"driver":"ienso_ethernet","name":"head_camera","type":"IensoCameraDevice"}],"config_files":{"CognexCameraDevice":{"url":"package:\\/\\/camera_io\\/config\\/sdk_cognex_camera_device.json"},"IensoCameraDevice":{"url":"package:\\/\\/camera_io\\/config\\/sdk_ienso_camera_device.json"}}},"props":{"device_types":["GenericDevice","CognexCameraDevice","CognexSimCameraDevice","IensoCameraDevice"],"library":"libio_plugins_camera_plugin.so","name":"SawyerInternalCameraPlugin","rev":1}})))
        return msg

    def _construct_config(self):
        msg = IODeviceConfiguration()
        msg.time = rospy.get_rostime()
        if not msg.time.secs:
            msg.time.secs = 1  # hack to ensure time is valid

        if self._name == 'head_camera':
            msg.commanded=json.dumps({"params":{"engine_params":{"autoEnroll":True,"configurable":False},"fps":10,"height":800,"image_path":"package:\/\/camera_io\/images\/ienso","image_source_id":"ienso_ethernet:EE:52:EB:4E:46:B8","sim_image":"Fiducials_marker1.png","tcp_port":8901,"width":1280},"ports":{"set_camera_resolution":{"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"owner":"device","rev":1,"type":"CameraResolution"}},"set_camera_stream_state":{"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_exposure":{"params":{"control":"exposure"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_flip":{"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_gain":{"params":{"control":"gain"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_mirror":{"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_window_x":{"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_window_y":{"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}}},"props":{"name":"IensoCamera","owner":"device","rev":1,"type":"IensoCameraDevice"},"signals":{"camera_streaming":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}},"resolution":{"params":{"default":[{"height":800,"width":1280}]},"props":{"format":{"role":"output","type":"object"},"owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}},"set_exposure":{"params":{"default":[-1],"range":{"max":[100],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}},"set_flip":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_flip","type":"IO"}},"set_gain":{"params":{"default":[0],"range":{"max":[79],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}},"set_mirror":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_mirror","type":"IO"}},"set_window_x":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_x","type":"IO"}},"set_window_y":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_y","type":"IO"}}}})
            msg.upgraded=json.dumps({"params":{"engine_params":{"autoEnroll":True,"configurable":False},"fps":10,"height":800,"image_path":"package:\/\/camera_io\/images\/ienso","image_source_id":"ienso_ethernet:EE:52:EB:4E:46:B8","sim_image":"Fiducials_marker1.png","tcp_port":8901,"width":1280},"ports":{"set_camera_resolution":{"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"owner":"device","rev":1,"type":"CameraResolution"}},"set_camera_stream_state":{"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_exposure":{"params":{"control":"exposure"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_flip":{"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_gain":{"params":{"control":"gain"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_mirror":{"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_window_x":{"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_window_y":{"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}}},"props":{"name":"IensoCamera","owner":"device","rev":1,"type":"IensoCameraDevice"},"signals":{"camera_streaming":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}},"resolution":{"params":{"default":[{"height":800,"width":1280}]},"props":{"format":{"role":"output","type":"object"},"owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}},"set_exposure":{"params":{"default":[-1],"range":{"max":[100],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}},"set_flip":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_flip","type":"IO"}},"set_gain":{"params":{"default":[0],"range":{"max":[79],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}},"set_mirror":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_mirror","type":"IO"}},"set_window_x":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_x","type":"IO"}},"set_window_y":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_y","type":"IO"}}}})
            msg.device.name = self._name
            msg.device.config = json.dumps({"params":{"engine_params":{"autoEnroll":True,"configurable":False},"fps":10,"height":800,"image_path":"package:\/\/camera_io\/images\/ienso","image_source_id":"ienso_ethernet:EE:52:EB:4E:46:B8","sim_image":"Fiducials_marker1.png","tcp_port":8901,"width":1280},"ports":{"set_camera_resolution":{"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"owner":"device","rev":1,"type":"CameraResolution"}},"set_camera_stream_state":{"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_exposure":{"params":{"control":"exposure"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_flip":{"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_gain":{"params":{"control":"gain"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_mirror":{"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_window_x":{"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_window_y":{"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}}},"props":{"id":"\/io\/internal_camera\/head_camera","name":"head_camera","owner":"device","rev":1,"type":"IensoCameraDevice"},"signals":{"camera_streaming":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}},"resolution":{"params":{"default":[{"height":800,"width":1280}]},"props":{"format":{"role":"output","type":"object"},"owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}},"set_exposure":{"params":{"default":[-1],"range":{"max":[100],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}},"set_flip":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_flip","type":"IO"}},"set_gain":{"params":{"default":[0],"range":{"max":[79],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}},"set_mirror":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_mirror","type":"IO"}},"set_window_x":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_x","type":"IO"}},"set_window_y":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_y","type":"IO"}}}})
            msg.ports.append(IOComponentConfiguration(name='set_camera_resolution',
              config=json.dumps({"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"name":"set_camera_resolution","owner":"device","rev":1,"type":"CameraResolution"}})))
            msg.ports.append(IOComponentConfiguration(name='set_camera_stream_state',
              config=json.dumps({"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"name":"set_camera_stream_state","owner":"device","rev":1,"type":"ControlOnOff"}})))
            msg.ports.append(IOComponentConfiguration(name='set_exposure',
              config=json.dumps({"params":{"control":"exposure"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"name":"set_exposure","owner":"device","rev":1,"type":"ControlMode"}})))
            msg.ports.append(IOComponentConfiguration(name='set_flip',
              config=json.dumps({"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"name":"set_flip","owner":"device","rev":1,"type":"ControlOnOff"}})))
            msg.ports.append(IOComponentConfiguration(name='set_gain',
              config=json.dumps({"params":{"control":"gain"},"props":{"format":{"data_type":"i16","role":"sink","type":"int"},"name":"set_gain","owner":"device","rev":1,"type":"ControlMode"}})))
            msg.ports.append(IOComponentConfiguration(name='set_mirror',
              config=json.dumps({"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"name":"set_mirror","owner":"device","rev":1,"type":"ControlOnOff"}})))
            msg.ports.append(IOComponentConfiguration(name='set_window_x',
              config=json.dumps({"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"name":"set_window_x","owner":"device","rev":1,"type":"ControlValue"}})))
            msg.ports.append(IOComponentConfiguration(name='set_window_y',
              config=json.dumps({"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"name":"set_window_y","owner":"device","rev":1,"type":"ControlValue"}})))

            msg.signals.append(IOComponentConfiguration(name='camera_streaming',
              config=json.dumps({"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"name":"camera_streaming","owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='resolution',
              config=json.dumps({"params":{"default":[{"height":800,"width":1280}]},"props":{"format":{"role":"output","type":"object"},"name":"resolution","owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_exposure',
              config=json.dumps({"params":{"default":[-1],"range":{"max":[100],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"name":"set_exposure","owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_flip',
              config=json.dumps({"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"name":"set_flip","owner":"device","rev":1,"sink":"set_flip","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_gain',
              config=json.dumps({"params":{"default":[0],"range":{"max":[79],"min":[-1]}},"props":{"format":{"role":"output","type":"int"},"name":"set_gain","owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_mirror',
              config=json.dumps({"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"name":"set_mirror","owner":"device","rev":1,"sink":"set_mirror","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_window_x',
              config=json.dumps({"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"name":"set_window_x","owner":"device","rev":1,"sink":"set_window_x","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_window_y',
              config=json.dumps({"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"name":"set_window_y","owner":"device","rev":1,"sink":"set_window_y","type":"IO"}})))
        else: # right_hand_camera
            msg.commanded=json.dumps({"params":{"command_service_timeout":1.0,"engine_params":{"autoEnroll":True,"configurable":False},"fps":10,"ftp_host":"port:203.0.113.130","height":480,"image_path":"package:\/\/camera_io\/images\/cognex","image_source_id":"cognex:00:D0:24:3D:2F:F6","sim_image":"Fiducials_marker1.png","tcp_port":8900,"width":752},"ports":{"set_brightness":{"params":{"control":"brightness"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_camera_resolution":{"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"owner":"device","rev":1,"type":"CameraResolution"}},"set_camera_stream_state":{"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_embedded_data":{"params":{},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"Data"}},"set_exposure":{"params":{"control":"exposure"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_flip":{"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_gain":{"params":{"control":"gain"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_mirror":{"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_strobe":{"params":{"control":"strobe"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlStrobe"}},"set_window_x":{"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_window_y":{"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}}},"props":{"name":"CognexCamera","owner":"device","rev":1,"type":"CognexCameraDevice"},"signals":{"camera_streaming":{"params":{"default":[True]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}},"resolution":{"params":{"default":[{"height":480,"width":752}]},"props":{"format":{"role":"output","type":"object"},"owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}},"set_brightness":{"params":{"default":[10],"range":{"max":[100],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_brightness","type":"RangedIO"}},"set_embedded_data":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_embedded_data","type":"IO"}},"set_exposure":{"params":{"default":[32],"range":{"max":[100],"min":[1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}},"set_flip":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_flip","type":"IO"}},"set_gain":{"params":{"default":[5],"range":{"max":[255],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}},"set_mirror":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_mirror","type":"IO"}},"set_strobe":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_strobe","type":"IO"}},"set_window_x":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_x","type":"IO"}},"set_window_y":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_y","type":"IO"}}}})
            msg.upgraded=json.dumps({"params":{"command_service_timeout":1.0,"engine_params":{"autoEnroll":True,"configurable":False},"fps":10,"ftp_host":"port:203.0.113.130","height":480,"image_path":"package:\/\/camera_io\/images\/cognex","image_source_id":"cognex:00:D0:24:3D:2F:F6","sim_image":"Fiducials_marker1.png","tcp_port":8900,"width":752},"ports":{"set_brightness":{"params":{"control":"brightness"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_camera_resolution":{"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"owner":"device","rev":1,"type":"CameraResolution"}},"set_camera_stream_state":{"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_embedded_data":{"params":{},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"Data"}},"set_exposure":{"params":{"control":"exposure"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_flip":{"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_gain":{"params":{"control":"gain"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_mirror":{"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_strobe":{"params":{"control":"strobe"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlStrobe"}},"set_window_x":{"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_window_y":{"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}}},"props":{"name":"CognexCamera","owner":"device","rev":1,"type":"CognexCameraDevice"},"signals":{"camera_streaming":{"params":{"default":[True]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}},"resolution":{"params":{"default":[{"height":480,"width":752}]},"props":{"format":{"role":"output","type":"object"},"owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}},"set_brightness":{"params":{"default":[10],"range":{"max":[100],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_brightness","type":"RangedIO"}},"set_embedded_data":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_embedded_data","type":"IO"}},"set_exposure":{"params":{"default":[32],"range":{"max":[100],"min":[1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}},"set_flip":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_flip","type":"IO"}},"set_gain":{"params":{"default":[5],"range":{"max":[255],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}},"set_mirror":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_mirror","type":"IO"}},"set_strobe":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_strobe","type":"IO"}},"set_window_x":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_x","type":"IO"}},"set_window_y":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_y","type":"IO"}}}})
            msg.device.name = self._name
            msg.device.config = json.dumps({"params":{"command_service_timeout":1.0,"engine_params":{"autoEnroll":True,"configurable":False},"fps":10,"ftp_host":"port:203.0.113.130","height":480,"image_path":"package:\/\/camera_io\/images\/cognex","image_source_id":"cognex:00:D0:24:3D:2F:F6","sim_image":"Fiducials_marker1.png","tcp_port":8900,"width":752},"ports":{"set_brightness":{"params":{"control":"brightness"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_camera_resolution":{"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"owner":"device","rev":1,"type":"CameraResolution"}},"set_camera_stream_state":{"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_embedded_data":{"params":{},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"Data"}},"set_exposure":{"params":{"control":"exposure"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlMode"}},"set_flip":{"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_gain":{"params":{"control":"gain"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_mirror":{"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlOnOff"}},"set_strobe":{"params":{"control":"strobe"},"props":{"format":{"role":"sink","type":"bool"},"owner":"device","rev":1,"type":"ControlStrobe"}},"set_window_x":{"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}},"set_window_y":{"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"owner":"device","rev":1,"type":"ControlValue"}}},"props":{"id":"\/io\/internal_camera\/right_hand_camera","name":"right_hand_camera","owner":"device","rev":1,"type":"CognexCameraDevice"},"signals":{"camera_streaming":{"params":{"default":[True]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}},"resolution":{"params":{"default":[{"height":480,"width":752}]},"props":{"format":{"role":"output","type":"object"},"owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}},"set_brightness":{"params":{"default":[10],"range":{"max":[100],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_brightness","type":"RangedIO"}},"set_embedded_data":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_embedded_data","type":"IO"}},"set_exposure":{"params":{"default":[32],"range":{"max":[100],"min":[1]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}},"set_flip":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_flip","type":"IO"}},"set_gain":{"params":{"default":[5],"range":{"max":[255],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}},"set_mirror":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_mirror","type":"IO"}},"set_strobe":{"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"owner":"device","rev":1,"sink":"set_strobe","type":"IO"}},"set_window_x":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_x","type":"IO"}},"set_window_y":{"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"owner":"device","rev":1,"sink":"set_window_y","type":"IO"}}}})
            msg.ports.append(IOComponentConfiguration(name='set_brightness',
              config=json.dumps({"params":{"control":"brightness"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"name":"set_brightness","owner":"device","rev":1,"type":"ControlValue"}})))
            msg.ports.append(IOComponentConfiguration(name='set_camera_resolution',
              config=json.dumps({"params":{"do_not_force":True},"props":{"format":{"role":"sink","type":"object"},"name":"set_camera_resolution","owner":"device","rev":1,"type":"CameraResolution"}})))
            msg.ports.append(IOComponentConfiguration(name='set_camera_stream_state',
              config=json.dumps({"params":{"control":"streaming"},"props":{"format":{"role":"sink","type":"bool"},"name":"set_camera_stream_state","owner":"device","rev":1,"type":"ControlOnOff"}})))
            msg.ports.append(IOComponentConfiguration(name='set_embedded_data',
              config=json.dumps({"params":{},"props":{"format":{"role":"sink","type":"bool"},"name":"set_embedded_data","owner":"device","rev":1,"type":"Data"}})))
            msg.ports.append(IOComponentConfiguration(name='set_exposure',
              config=json.dumps({"params":{"control":"exposure"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"name":"set_exposure","owner":"device","rev":1,"type":"ControlMode"}})))
            msg.ports.append(IOComponentConfiguration(name='set_flip',
              config=json.dumps({"params":{"control":"flip"},"props":{"format":{"role":"sink","type":"bool"},"name":"set_flip","owner":"device","rev":1,"type":"ControlOnOff"}})))
            msg.ports.append(IOComponentConfiguration(name='set_gain',
              config=json.dumps({"params":{"control":"gain"},"props":{"format":{"data_type":"u8","role":"sink","type":"int"},"name":"set_gain","owner":"device","rev":1,"type":"ControlValue"}})))
            msg.ports.append(IOComponentConfiguration(name='set_mirror',
              config=json.dumps({"params":{"control":"mirror"},"props":{"format":{"role":"sink","type":"bool"},"name":"set_mirror","owner":"device","rev":1,"type":"ControlOnOff"}})))
            msg.ports.append(IOComponentConfiguration(name='set_strobe',
              config=json.dumps({"params":{"control":"strobe"},"props":{"format":{"role":"sink","type":"bool"},"name":"set_strobe","owner":"device","rev":1,"type":"ControlStrobe"}})))
            msg.ports.append(IOComponentConfiguration(name='set_window_x',
              config=json.dumps({"params":{"control":"window_x"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"name":"set_window_x","owner":"device","rev":1,"type":"ControlValue"}})))
            msg.ports.append(IOComponentConfiguration(name='set_window_y',
              config=json.dumps({"params":{"control":"window_y"},"props":{"format":{"data_type":"u16","role":"sink","type":"int"},"name":"set_window_y","owner":"device","rev":1,"type":"ControlValue"}})))

            msg.signals.append(IOComponentConfiguration(name='camera_streaming',
              config=json.dumps({"params":{"default":[True]},"props":{"format":{"role":"output","type":"bool"},"name":"camera_streaming","owner":"device","rev":1,"sink":"set_camera_stream_state","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='resolution',
              config=json.dumps({"params":{"default":[{"height":480,"width":752}]},"props":{"format":{"role":"output","type":"object"},"name":"resolution","owner":"device","rev":1,"sink":"set_camera_resolution","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_brightness',
              config=json.dumps({"params":{"default":[10],"range":{"max":[100],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"name":"set_brightness","owner":"device","rev":1,"sink":"set_brightness","type":"RangedIO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_embedded_data',
              config=json.dumps({"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"name":"set_embedded_data","owner":"device","rev":1,"sink":"set_embedded_data","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_exposure',
              config=json.dumps({"params":{"default":[32],"range":{"max":[100],"min":[1]}},"props":{"format":{"role":"output","type":"int"},"name":"set_exposure","owner":"device","rev":1,"sink":"set_exposure","type":"RangedIO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_flip',
              config=json.dumps({"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"name":"set_flip","owner":"device","rev":1,"sink":"set_flip","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_gain',
              config=json.dumps({"params":{"default":[5],"range":{"max":[255],"min":[0]}},"props":{"format":{"role":"output","type":"int"},"name":"set_gain","owner":"device","rev":1,"sink":"set_gain","type":"RangedIO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_mirror',
              config=json.dumps({"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"name":"set_mirror","owner":"device","rev":1,"sink":"set_mirror","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_strobe',
              config=json.dumps({"params":{"default":[False]},"props":{"format":{"role":"output","type":"bool"},"name":"set_strobe","owner":"device","rev":1,"sink":"set_strobe","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_window_x',
              config=json.dumps({"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"name":"set_window_x","owner":"device","rev":1,"sink":"set_window_x","type":"IO"}})))
            msg.signals.append(IOComponentConfiguration(name='set_window_y',
              config=json.dumps({"params":{"default":[0]},"props":{"format":{"role":"output","type":"int"},"name":"set_window_y","owner":"device","rev":1,"sink":"set_window_y","type":"IO"}})))

        return msg

    def _construct_state(self, streaming_state, cmd_time=None):
        msg = IODeviceStatus()
        msg.time = rospy.get_rostime()
        if not msg.time.secs:
            msg.time.secs = 1  # hack to ensure time is valid

        msg.device = IOComponentStatus(name=self._name,
            status=IOStatus(tag="ready", id='', detail=json.dumps("")))
        msg.signals.append(IODataStatus(name="camera_streaming",
            format=json.dumps({"role":"output","type":"bool"}),
            data=json.dumps([streaming_state]),
            status=IOStatus(tag="ready", id='', detail=json.dumps(""))
            ))
        if cmd_time:
            self._cmd_times.append(cmd_time)
            self._cmd_times = self._cmd_times[-100:]
            msg.commands = self._cmd_times
        return msg

    def _handle_command(self, msg):
        """
        command topic callback to echo streaming state
        """
        is_streaming = json.loads(msg.args)["signals"]["camera_streaming"]["data"][0]
        self._state_pub.publish(self._construct_state(is_streaming, cmd_time=msg.time))

if __name__ == "__main__":
    rospy.init_node("camera_io_sim")
    rhc = CameraIOSim("right_hand_camera", is_streaming = True)
    hc = CameraIOSim("head_camera", is_streaming = False)
    rospy.spin()
